package novemberBot.utils;

/**Trig contains static helper methods useful to many other classes
 * @author Antoine Lennartz*/
public class Trig {
	
	public static final double DEG_TO_RAD=Math.PI/180.0;
	
	/**
	 * @param angle - an angle in degrees
	 * 
	 * @return an angle in degrees between 0 and 360*/
	public static double fixAngle(double angle) {
			angle=angle%360;
			if(angle<0) angle+=360;
			return angle;
	}
	
	/**@param angle -  an angle in degrees
	 * @return an angle between -180 and 180*/
	public static double fixAngle2(double angle){
		angle = fixAngle(angle);
		if(angle>180) angle-=360;
		return angle;
		
	}
	/**returns magnitude of 2d vector*/
	public static double magnitude(double[] vect){
		return Math.sqrt(vect[0]*vect[0] + vect[1]*vect[1]);
	}
	
	public static int round(double d){
		if(d-((int) d)<0.5){
			return (int)d;
		}else return (int)d+1;
	}
	
	/**@param a - angle in degrees
	 * @param b - angle in degrees 
	 * @return the minimal angle from a to b */
	/*public static double minimumAngleFromTo(double a, double b) {
		double d = fixAngle(b - a);
		
		if (d < 180.0)
			return d;
		else
			return d - 360.0;
	}*/
	
	/**
	 * @param theta - an angle in degrees
	 * @return the sin of theta*/
	public static double sin2(double theta){
		return Math.sin(theta*DEG_TO_RAD);
	}
	
	/**
	 * @param theta - an angle in degrees
	 * @return the cos of theta*/
	public static double cos2(double theta){
		return Math.cos(theta*DEG_TO_RAD);
	}
	
	/**
	 * @param x - x position on grid
	 * @param y - y position on grid
	 * @return theta: angle in degrees from origin to point(x,y) with angle defined in compass fashion*/
	public static double atan3(double x, double y){
		double theta=Math.atan(x/y)*180/Math.PI;
		if(x>0){
			if(y<0){
				theta+=180;
			}
		}else{
			theta+=360;
			if(y<0){
				theta-=180;
			}
		}
		return theta;
	}
	/**works if points are position vectors from odometer as pos[2] is ignored
	 * @param pointA - [x,y] coordinates of point
	 * @param pointB - [x,y] coordinates of point
	 * @return distance from pointA to pointB*/
	public static double distance(double[] pointA, double[] pointB){
		return Math.sqrt( (pointA[0]-pointB[0])*(pointA[0]-pointB[0]) + (pointA[1]-pointB[1])*(pointA[1]-pointB[1]) );
	}
	/**for short arrays that need sorting doesnt have anything to do with trig but ...
	 * @param array - the array to be sorted*/
	public static void bubbleSort(double[] array){
		double tmp;
		for(int j=array.length-1; j>0; j--){
			for(int i=0; i<j; i++){
				if(array[i]>array[i+1]){
					tmp=array[i+1];
					array[i+1]=array[i];
					array[i]=tmp;
				}
			}
		}
	}
	/**nearest grid Crossing coordinates, preserves heading
	 * @param cPos - position close to the grid crossing*/
	public static double[] nearestCross(double[] cPos){
		return new double[]{Math.round(cPos[0]/30.48)*30.48, Math.round(cPos[1]/30.48)*30.48, cPos[2]};
	}
}
